/****************************************************************************
 *
 *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file test_blocking_list.cpp
 * Unit tests for BlockingList (thread-safe intrusive sorted list)
 */

#include "px4_platform_common/containers/blocking_list.hpp"
#include "FreeRTOS.h"
#include "task.h"
#include <string.h>
#include <stddef.h>

// Forward declaration of debug_printf from main.c
extern "C" int debug_printf(const char *format, ...);

// Test counters
static int test_passed = 0;
static int test_failed = 0;

// Test assertion macro
#define TEST_ASSERT(condition, message) \
	do { \
		if (condition) { \
			test_passed++; \
		} else { \
			test_failed++; \
			debug_printf("  ❌ FAIL: %s\n", message); \
		} \
	} while (0)

// Test item structure
typedef struct {
	intrusive_sorted_list_node_t node;
	int value;
	char name[32];
} TestItem;

// Comparison function for sorting by value
static bool compare_by_value(const void *a, const void *b)
{
	const TestItem *item_a = (const TestItem *)a;
	const TestItem *item_b = (const TestItem *)b;
	return item_a->value <= item_b->value;
}

// Comparison function for sorting by name
static bool compare_by_name(const void *a, const void *b)
{
	const TestItem *item_a = (const TestItem *)a;
	const TestItem *item_b = (const TestItem *)b;
	return strcmp(item_a->name, item_b->name) <= 0;
}

// Test 1: Basic operations
static void test_basic_operations()
{
	debug_printf("Test 1: Basic operations\n");

	BlockingList list;
	list.init(offsetof(TestItem, node), compare_by_value);

	TEST_ASSERT(list.size() == 0, "List should be empty initially");

	TestItem item1 = { .value = 10 };
	strcpy(item1.name, "item1");

	list.add(&item1);
	TEST_ASSERT(list.size() == 1, "List should have 1 item after add");

	bool removed = list.remove(&item1);
	TEST_ASSERT(removed == true, "Remove should return true");
	TEST_ASSERT(list.size() == 0, "List should be empty after remove");

	removed = list.remove(&item1);
	TEST_ASSERT(removed == false, "Remove non-existent item should return false");
}

// Test 2: Multiple items
static void test_multiple_items()
{
	debug_printf("Test 2: Multiple items\n");

	BlockingList list;
	list.init(offsetof(TestItem, node), compare_by_value);

	TestItem item1 = { .value = 30 };
	TestItem item2 = { .value = 10 };
	TestItem item3 = { .value = 20 };

	list.add(&item1);
	list.add(&item2);
	list.add(&item3);

	TEST_ASSERT(list.size() == 3, "List should have 3 items");

	list.clear();
	TEST_ASSERT(list.size() == 0, "List should be empty after clear");
}

// Test 3: Sorted insertion
static void test_sorted_insertion()
{
	debug_printf("Test 3: Sorted insertion\n");

	BlockingList list;
	list.init(offsetof(TestItem, node), compare_by_name);

	TestItem item1, item2, item3;
	strcpy(item1.name, "charlie");
	strcpy(item2.name, "alice");
	strcpy(item3.name, "bob");

	list.add(&item1);
	list.add(&item2);
	list.add(&item3);

	TEST_ASSERT(list.size() == 3, "List should have 3 items");

	// Items should be sorted: alice, bob, charlie
	// We can't easily verify order without exposing internals,
	// but we trust the underlying intrusive_sorted_list
	TEST_ASSERT(true, "Items added in sorted order");
}

// Test 4: Thread safety - concurrent adds
static BlockingList *g_test_list = NULL;
static volatile int g_add_count = 0;

static void add_task(void *param)
{
	TestItem *item = (TestItem *)param;

	for (int i = 0; i < 10; i++) {
		g_test_list->add(item);
		vTaskDelay(pdMS_TO_TICKS(1));
		g_test_list->remove(item);
		g_add_count++;
	}

	vTaskDelete(NULL);
}

static void test_thread_safety()
{
	debug_printf("Test 4: Thread safety\n");

	BlockingList list;
	list.init(offsetof(TestItem, node), compare_by_value);
	g_test_list = &list;
	g_add_count = 0;

	TestItem item1 = { .value = 10 };
	TestItem item2 = { .value = 20 };

	TaskHandle_t task1, task2;

	xTaskCreate(add_task, "add1", 512, &item1, 10, &task1);
	xTaskCreate(add_task, "add2", 512, &item2, 10, &task2);

	// Wait for tasks to complete (20 operations each = 40 total)
	vTaskDelay(pdMS_TO_TICKS(500));

	TEST_ASSERT(g_add_count == 20, "Both tasks should complete 10 operations each");
	TEST_ASSERT(list.size() == 0, "List should be empty after all adds/removes");

	g_test_list = NULL;
}

// Test 5: Mutex access
static void test_mutex_access()
{
	debug_printf("Test 5: Mutex access\n");

	BlockingList list;
	list.init(offsetof(TestItem, node), compare_by_value);

	SemaphoreHandle_t &mutex = list.mutex();
	TEST_ASSERT(mutex != NULL, "Mutex should not be NULL");

	// Test manual locking
	BaseType_t result = xSemaphoreTake(mutex, portMAX_DELAY);
	TEST_ASSERT(result == pdTRUE, "Should be able to take mutex");

	xSemaphoreGive(mutex);
	TEST_ASSERT(true, "Should be able to give mutex");
}

// Test 6: Large number of items
static void test_large_list()
{
	debug_printf("Test 6: Large number of items\n");

	BlockingList list;
	list.init(offsetof(TestItem, node), compare_by_value);

	const int NUM_ITEMS = 50;
	TestItem items[NUM_ITEMS];

	// Add items in reverse order
	for (int i = NUM_ITEMS - 1; i >= 0; i--) {
		items[i].value = i;
		list.add(&items[i]);
	}

	TEST_ASSERT(list.size() == NUM_ITEMS, "List should have all items");

	// Remove all items
	for (int i = 0; i < NUM_ITEMS; i++) {
		bool removed = list.remove(&items[i]);
		TEST_ASSERT(removed == true, "Each item should be removed successfully");
	}

	TEST_ASSERT(list.size() == 0, "List should be empty after removing all items");
}

// Test 7: Duplicate values
static void test_duplicate_values()
{
	debug_printf("Test 7: Duplicate values\n");

	BlockingList list;
	list.init(offsetof(TestItem, node), compare_by_value);

	TestItem item1 = { .value = 10 };
	TestItem item2 = { .value = 10 };
	TestItem item3 = { .value = 10 };

	list.add(&item1);
	list.add(&item2);
	list.add(&item3);

	TEST_ASSERT(list.size() == 3, "List should have 3 items with duplicate values");

	list.remove(&item2);
	TEST_ASSERT(list.size() == 2, "List should have 2 items after removing one");

	list.clear();
	TEST_ASSERT(list.size() == 0, "List should be empty after clear");
}

// Test 8: Empty list operations
static void test_empty_list()
{
	debug_printf("Test 8: Empty list operations\n");

	BlockingList list;
	list.init(offsetof(TestItem, node), compare_by_value);

	TEST_ASSERT(list.size() == 0, "New list should be empty");

	TestItem item = { .value = 10 };
	bool removed = list.remove(&item);
	TEST_ASSERT(removed == false, "Remove from empty list should return false");

	list.clear();
	TEST_ASSERT(list.size() == 0, "Clear on empty list should work");
}

// Main test function
extern "C" void test_blocking_list()
{
	debug_printf("\n========================================\n");
	debug_printf("  BlockingList Tests\n");
	debug_printf("========================================\n");

	test_passed = 0;
	test_failed = 0;

	test_basic_operations();
	test_multiple_items();
	test_sorted_insertion();
	test_thread_safety();
	test_mutex_access();
	test_large_list();
	test_duplicate_values();
	test_empty_list();

	debug_printf("========================================\n");
	debug_printf("  Test Summary\n");
	debug_printf("========================================\n");
	debug_printf("Total tests: %d\n", test_passed + test_failed);
	debug_printf("Passed: %d\n", test_passed);
	debug_printf("Failed: %d\n", test_failed);

	if (test_failed == 0) {
		debug_printf("\n✅ All BlockingList tests PASSED!\n");
	} else {
		debug_printf("\n❌ Some BlockingList tests FAILED!\n");
	}

	debug_printf("========================================\n\n");
}

